/*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*\
|          Copyright (C) 2013 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |
                                                                                
     This program is free software: you can redistribute it and/or modify       
     it under the terms of the GNU General Public License version 2 as          
     published by the Free Software Foundation.                                 
                                                                                
     This program is distributed in the hope that it will be useful,            
     but WITHOUT ANY WARRANTY; without even the implied warranty of             
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              
     GNU General Public License for more details.                               
                                                                                
     You should have received a copy of the GNU General Public License          
     along with this program.  If not, see <http://www.gnu.org/licenses/>.      
                                                                                
            Jack Elston                                                         
|           elstonj@colorado.edu                                               |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/
#include "udp_simulink.h"

//---------------------------------------------------------------------------
UDPSimulink::UDPSimulink(String name) : pThread(name)
{
}

// --------------------------------------------------------------------------
bool UDPSimulink::init(ShmStruct * shmStructPtr)
{
  bool retval = pThread::init(shmStructPtr);

	initUDP(shmPtr->udpsocket_status.portStr,
					Socket::UDP,
					Socket::DUPLEX,
					shmPtr->udpsocket_status.hostStr);

	return retval;
}

// --------------------------------------------------------------------------
UDPSimulink::~UDPSimulink()
{
	if( sock != NULL )
		sock->close();
}

// --------------------------------------------------------------------------
void UDPSimulink::initUDP(String port,
		Socket::SocketType type,
		Socket::SocketMode mode,
		String host
		)
{
	sock = NULL;

	sockType = type;
	sockMode = mode;

	if( sock != NULL ) {
		sock->close();
		delete sock;
		sock = NULL;
	}

	sock = new Socket(host.c_str(), port.c_str(), type);
	if( sock == NULL ) {
		//cout << "UDPSimulink::initUDP() - could not get new socket, " <<  strerror(errno) << endl;
		setStatus("Could not get new Socket");
		int errsv = errno;
		setError(errsv);
		return;
	} else {
		sock->setNonBlocking();
		setStatus("Created Socket");
	}
}

// --------------------------------------------------------------------------
bool UDPSimulink::checkSocketConnection(void)
{
	/*<---Check Socket Conn--->*/
	if( sock->isClosed() ) {
		if ( !sock->open( sockMode ) ) {
			setStatus("Could not open Socket");

			// timeout for a second and try again 
			sleep(1); 
			return false;
		} 
	} 

	if(!sock->isConnected()) {
		setStatus("Not Connected");
		sock->connectHost();
	}

	if(!sock->isConnected()) {
		sleep(1); 
		return false;
	}

	setStatus("OK");
	return true;
}

// --------------------------------------------------------------------------
void UDPSimulink::update(void)
{
	if(!checkSocketConnection()) return;

	system_t* status;
	uint8_t* dataPtr = NULL;
	size_t size = 0;
	bool read = false;
	bool send = false;
	PACKET_TYPE type;

	if(readUDP() > 0) {
		shmPtr->udpsocket_status.rx ++;  // counting full message sets 
		type = (PACKET_TYPE)(buffer[0]);

		switch (type) {
			case AIR_PKT:
				status = &(shmPtr->packet_status.air);
				dataPtr = (uint8_t *)(&(shmPtr->piccolo_rx.air));
				size = sizeof(airStruct_t);
				read = true;
				break;
			case IMU_PKT:
				status = &(shmPtr->packet_status.imu);
				dataPtr = (uint8_t *)(&(shmPtr->piccolo_rx.imu));
				size = sizeof(imuStruct_t);
				read = true;
				break;
			case ENG_PKT:
				status = &(shmPtr->packet_status.engine);
				dataPtr = (uint8_t *)(&(shmPtr->piccolo_rx.engine));
				size = sizeof(engineStruct_t);
				read = true;
				break;
			case GPS_PKT:
				status = &(shmPtr->packet_status.gps);
				dataPtr = (uint8_t *)(&(shmPtr->piccolo_rx.gps));
				size = sizeof(gpsStruct_t);
				read = true;
				break;
			default:
				char temp[32];
				sprintf(temp,"No such type %d",type);
				setStatus(temp);
				break;
		}
	}

	if(read) {
		mutex_unlock();
		if(status->ready)
			shmPtr->udpsocket_status.dropped ++;
		memcpy(dataPtr,buffer + UDP_HEADER_SIZE,size);
		status->ready = true;
		mutex_lock();
	}

	if(shmPtr->packet_status.servo.ready) {
		type = SERVO_PKT; send = true;
		status = &(shmPtr->packet_status.servo);
	  size = sizeof(servoStruct_t) * 16;}
	if(shmPtr->packet_status.conf.ready) {
		type = CONF_PKT; send = true;
		status = &(shmPtr->packet_status.conf);
	  size = sizeof(confStruct_t);}

	if(send) {
		if(writeUDP(type) == (size + UDP_HEADER_SIZE)) {
			mutex_unlock();
			shmPtr->udpsocket_status.tx ++; // counting messages rather than bytes;
			currentStamp.stamp();
			status->rate = SEC2MICRO / (currentStamp - status->time);
			status->time.stamp();
			status->ready = false;
			mutex_lock();
		}
	}

	if((shmPtr->udpsocket_status.dropped) > 0) {
		char temp[32];
		sprintf(temp,"Dropped %d packets",shmPtr->udpsocket_status.dropped,type);
		setStatus(temp);
	}

}

//---------------------------------------------------------------------------
int UDPSimulink::readUDP()
{
	int n = sock->read(buffer, BUFFER_SIZE);
	return n;
}

//---------------------------------------------------------------------------
int UDPSimulink::writeUDP(PACKET_TYPE type)
{

	size_t size = 0;
	uint8_t* dataPtr = NULL;
	switch(type) {
		case SERVO_PKT:
			size = sizeof(servoStruct_t) * 16;
			dataPtr = (uint8_t* )(shmPtr->piccolo_tx.servos);
			break;
		case CONF_PKT:
			size = sizeof(confStruct_t);
			dataPtr = (uint8_t* )(&(shmPtr->piccolo_tx.config));
			break;
		default:
			return 0;
	}

	int n = 0;
	if(! sock->isClosed() && dataPtr != NULL) {
		buffer[0] = type;
		memcpy(buffer + UDP_HEADER_SIZE,dataPtr,size);
		in_addr host;
		host.s_addr = inet_addr(shmPtr->udpsocket_status.hostStr.c_str());
		n = sock->write(host, buffer, size + UDP_HEADER_SIZE);
	}
	return n;
}

//---------------------------------------------------------------------------
void UDPSimulink::setStatus(const char* status) {
	mutex_unlock();
	shmPtr->udpsocket_status.state = status;
	mutex_lock();
}

//---------------------------------------------------------------------------
void UDPSimulink::setError(int errsv) {
	mutex_unlock();
	shmPtr->udpsocket_status.error = errsv;
	mutex_lock();
}
